import magnet_plate_robot as mrobot
import atexit
import time

try:
    Robot = mrobot.Magnet_Plate_Robot()
except Exception as e:
    input(f"机器人启动失败, 可能是未检测到串口连接.错误信息：\n{e}")
    exit()

atexit.register(lambda: Robot.set_rotation(0))

def main():
    while True:
        command = input("Input command: ")
        command_dealer(command)

def command_dealer(cmd:str, run=True):
    ls = cmd.split()
    legal = False
    if len(ls) > 4:
        if run:
            print("Unknown command")
    else:
        if ls[0] in ["move", "m", "mv"] and ((len(ls) == 4) or (len(ls) == 3)):
            try:
                x = float(ls[1])
                y = float(ls[2])
                spd = float(ls[3]) if len(ls) == 4 else 100
                legal = True
            except Exception as e:
                if run:
                    print(e)
                return legal, e
            if run:
                print("moveing ...")
                Robot.move((x, y), spd)
                print(f"move done, now pos at {Robot.position[0]:.2f}, {Robot.position[1]:.2f}")
        if ls[0] in ["moveto", "mt", "mvt"] and ((len(ls) == 4) or (len(ls) == 3)):
            try:
                x = float(ls[1])
                y = float(ls[2])
                spd = float(ls[3]) if len(ls) == 4 else 100
                legal = True
            except Exception as e:
                if run:
                    print(e)
                return legal, e
            if run:
                print("moveing ...")
                Robot.move_to((x, y), spd)
                print(f"move done, now pos at {Robot.position[0]:.2f}, {Robot.position[1]:.2f}")
        if ls[0] in ["rotate", "r", "rt"] and (len(ls) == 2):
            try:
                spd = float(ls[1])
                legal = True
            except Exception as e:
                if run:
                    print(e)
                return legal, e
            if run:
                Robot.set_rotation(spd)
                print(f"rotate speed now {spd} rps")

        if ls[0] in ["sleep", "s", "wait"] and (len(ls) == 2):
            try:
                tm = float(ls[1])
                legal = True
            except Exception as e:
                if run:
                    print(e)
                return legal, e
            if run:
                print(f"wait for {spd} s")
                time.sleep(tm)            
        
        if ls[0] in ["exit", "esc", "quit", "q"]:
            legal = True
            if run:
                Robot.set_rotation(0)
                quit()
        return legal, None


if __name__ == "__main__":
    try:
        main()
    except Exception as e:
        input(f"机器人遇到了问题.错误信息：\n{e}")